import time
import utime
import lcd
import image
import sensor
import socket
import KPU as kpu
from Maix import GPIO
from machine import Timer,PWM
from fpioa_manager import fm
from Maix import FPIOA
fpioa = FPIOA()

#PWM通过定时器配置，接到IO17引脚（Pin IO17）
tim_PWM = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim1_PWM = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S1 = PWM(tim_PWM, freq=50, duty=0, pin=15)#定义的上舵机控制对象
S2 = PWM(tim1_PWM, freq=50, duty=0, pin=17)#定义的下舵机控制对象

Timing_time = time.ticks_us()           #
distance = 0               #HC_SR04所测得距离

up_servo_value = 50        #上舵机中值50
down_servo_value = 16     #下舵机中值-24
PD = {'KP':1.9,'KD':0.9}  #PD控制的PD参数
now_error = {'x':0,'y':0}  #PD控制所需的当前误差
last_error = {'x':0,'y':0} #PD控制所需的上一次误差

Task_run_flag = [0,0]#分时执行标志，多一条线程在列表里多加一个0元素
Task_time_value_ms = [100,20]#分时执行间隔时间，多一条线程在列表里多加一个间隔时间的元素
Task_Itvtime_value_ms = Task_time_value_ms#分时重装间隔元素

times = 0

SSID = "HUAWEI MatePad"
PASW = "qwerasdz"

fpioa.set_function(11, fm.fpioa.GPIOHS11)
fpioa.set_function(13, fm.fpioa.GPIOHS13)
fpioa.set_function(14, fm.fpioa.GPIOHS14)
fpioa.set_function(12, fm.fpioa.GPIO0)#激光灯与蜂鸣器控制引脚
fpioa.set_function(16, fm.fpioa.GPIO1)#HC_SR04的Trig引脚


R = GPIO(GPIO.GPIOHS11, GPIO.IN, GPIO.PULL_UP)#HC_SR04的Echo引脚
K1 = GPIO(GPIO.GPIOHS13, GPIO.IN,GPIO.PULL_UP)
K2 = GPIO(GPIO.GPIOHS14, GPIO.IN,GPIO.PULL_UP)
C = GPIO(GPIO.GPIO0, GPIO.OUT)
T = GPIO(GPIO.GPIO1,GPIO.OUT)
T.value(0)
C.value(0)

lcd.init()

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((224, 224))#采集图像大小
sensor.set_hmirror(False)#镜像
sensor.set_vflip(False)
sensor.run(1)

def Servo(servo,angle):#舵机驱动函数
    servo.duty((angle+90)/180*10+2.5)

def Get_distance ():
    T.value(1)
    time.sleep_us(10)
    T.value(0)
    time.sleep_ms(30)

def HC_SR04 (pin_num):#外部中断回调函数
    global Timing_time,distance
    if pin_num.value() == 1:                  #判断是上升沿
        Timing_time = time.ticks_us()
    else:                                 #判断是下降沿
        Timing_time = time.ticks_us() - Timing_time
        distance = (Timing_time*0.034)/2   #计算距离CM
#        print(distance)

def Task_Timer (timer):#分时任务
    global Task_run_flag,Task_time_value_ms,Task_Itvtime_value_ms
    for i in range(len(Task_time_value_ms)):
        Task_time_value_ms[i]-=1
        if Task_time_value_ms[i] <= 0:
            Task_run_flag[i] = 1
            Task_time_value_ms[i] = Task_Itvtime_value_ms[i]

def Task_Exe():#分时执行
    global Task_run_flag
    for i in range(len(Task_run_flag)):
        if Task_run_flag[0] == 1:
            Get_distance()
            Task_run_flag[0] == 0
        if Task_run_flag[1] == 1:
#            Key ();
            Task_run_flag[1] = 0

#外部引脚中断配置
R.irq(HC_SR04,GPIO.IRQ_BOTH, GPIO.WAKEUP_NOT_SUPPORT,1)
#分时执行定时器配置
tim_Task = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=1, unit=Timer.UNIT_MS, callback=Task_Timer, arg=None, start=True, priority=2, div=0)

labels = ["basketball", "circle", "football", "rectangle", "triangle", "volleyball"]
anchors = [3.4375, 3.40625, 2.65625, 2.5625, 4.78125, 4.71875, 2.15625, 2.09375, 1.8125000000000002, 1.8125000000000002]
model_addr="/sd/m.kmodel"

task = kpu.load(model_addr)
kpu.init_yolo2(task, 0.7, 0.3, 5, anchors) # threshold:[0,1], nms_value: [0, 1]

Servo(S1,up_servo_value)
Servo(S2,down_servo_value)

while True :
    Task_Exe()
    img = sensor.snapshot()
    objects = kpu.run_yolo2(task, img)
    if objects:
        times = times+1
        pos = objects[0].rect()
        statis = img.get_statistics(roi=pos)#统计图片中的数据
#        print(image.lab_to_rgb([statis.l_mode(),statis.a_mode(),statis.b_mode()]))
        x = pos[0]+pos[2]/2
        y = pos[1]+pos[3]/2
        RGB = img.get_pixel(int(x),int(y))
        now_error['x'] = (112-x)
        now_error['y'] = (138-y)
        up_servo_value -= (0.1*(PD['KP']*now_error['y']+PD['KD']*(last_error['y']-now_error['y'])))
        if up_servo_value >= 80 : up_servo_value = 80#做限幅
        if up_servo_value <= 20 : up_servo_value = 20
        Servo(S1,up_servo_value)    #上
        down_servo_value += (0.1*(PD['KP']*now_error['x']+PD['KD']*(last_error['x']-now_error['x'])))
        if down_servo_value >= 16 : down_servo_value = 16
        if down_servo_value <= -64 : down_servo_value = -64
        Servo(S2,down_servo_value)   #下

        if now_error['x']<5 and now_error['y']<5 and now_error['x']>-5 and now_error['y']>-5:
            C.value(1)
        elif now_error['x']>5 and now_error['y']>5 and now_error['x']<-5 and now_error['y']<-5:
            C.value(0)
#        print(objects[0].classid())
        if objects[0].classid() == 4 or objects[0].classid() == 3:
            img.draw_string(pos[0], pos[1]+40, "sides:%0.3f"%float(pos[2]*distance*0.02818),color=(128,0,0), scale=2)
        else:
            img.draw_string(pos[0], pos[1]+40, "diameter:%0.3f"%float(pos[2]*distance*0.02818),color=(128,0,0), scale=2)

        img.draw_string(pos[0], pos[1]+20, "L:%d,A:%d,B:%d"%(statis.l_mode(),statis.a_mode(),statis.b_mode()),color=(128,0,0), scale=2)
        img.draw_rectangle(pos)#画矩形框  （pos[0],pos[1]）矩形左上角坐标（pos[2],pos[3]）矩形右下角坐标
        img.draw_string(pos[0], pos[1], "%s : %.2f" %(labels[objects[0].classid()], objects[0].value()), scale=2, color=(255,0,0))
        img.draw_string(40, 190, "distance:%0.3f"%distance,color=(128,0,0), scale=1)
    elif times<5:
        C.value(0)
        down_servo_value = down_servo_value - 1
        if down_servo_value <= -64 : down_servo_value = -64
        Servo(S2,down_servo_value)
        time.sleep_ms(50)

    img.draw_string(40, 200, "down_servo_value:%f"%down_servo_value,color=(128,0,0), scale=1)
    img.draw_string(40, 210, "up_servo_value:%f"%up_servo_value, color=(128,0,0),scale=1)
    lcd.display(img)















